/* ***********************************************************************
 * img_stab_if_R.cpp
 * ABOUT : Implement the class of Image stabilization.
 * AUTHOR : Dilawar Singh
 * Created on : April 15, 2010
 * Modified log :
 April 15, 2010
 April 28, 2010
 June 09, 2010 : Modified for adjusting cropping parameters.


 (c) Dilawar Singh, 2010
 *************************************************************************/

#include "types.h"
#include "img_stab_if_R.h"
#include <iostream>
#include <vector>
#include <cmath>

video_handler* img_stab_if_R::pBaseVideoHandler;

/*
 * Constructor.
 */
img_stab_if_R::img_stab_if_R()
{
    for(int i=0; i < 2*QUAT_LENGTH; i++)
    {
	arrayTwoQuaternion[i] = 0;
    }

    pObjCamMotionManager = new cam_motion_mgr();
}
img_stab_if_R::~img_stab_if_R()
{
    if(pObjCamMotionManager)
	delete pObjCamMotionManager;
}

//Set Base Interface at startup
int img_stab_if_R::initInterface(video_handler* pVideoHandler)
{
    pBaseVideoHandler = pVideoHandler;
    return(true);
}

img_stab_if_R::img_stab_if_R(kalmanType *pArrayQuat, kalmanType *pArrayCamMat) : myParamVec(4*WINDOW_SIZE), myFilteredVec(4), myVecS(WINDOW_SIZE)
{
    for(int i = 0; i < 2*QUAT_LENGTH; i++)
    {
	arrayTwoQuaternion[i] = *pArrayQuat;
	pArrayQuat++;
    }

    for(int i=0; i < (2*K_MATRIX_LENGTH); i++)
    {
	arrayCamInternalMatrix[i] = *pArrayCamMat;
	pArrayCamMat++;
    }

    pObjCamMotionManager = new cam_motion_mgr();
}

/*
 * This function will accumulate the parameter in a queue.
 */
vector<kalmanType> img_stab_if_R::accParamsInQueue(kalmanType *pMyArray, vector<kalmanType> myParamVec)
{

#if 0     
    for(vector<kalmanType>::size_type  i =0; i < myParamVec.size() ; i++)
    {
	cout<<"\t"<<myParamVec[i]<<"\t"<<i;
    }
#endif

    if(myParamVec.size() >= 4*WINDOW_SIZE)
    {
	while(myParamVec.size() > 4*WINDOW_SIZE)
	{
	    myParamVec.erase(myParamVec.begin());
	}
    }
    else
    {
	while(myParamVec.size() < 4*WINDOW_SIZE)
	{
	    myParamVec.push_back(0);
	}
    }

    for(int i =0; i < 4; i++)
    {
	myParamVec.push_back(*pMyArray);
	pMyArray++;
    }

#if 0
    cout<<"\n--DEBUG: print array...\n"<<endl;
    cout<<"\n**************************************"<<endl;
    for(vector<kalmanType>::size_type  i =0; i < myParamVec.size() ; i++)
    {
	cout<<"\t"<<myParamVec[i];
    }
#endif

    return myParamVec;
}


/* 
 *Caculate the new values of s theta and t based on these values. 
 */
vector<kalmanType> img_stab_if_R::calcSmoothMotion(vector<kalmanType> myParamVec)
{
    //create intermediate array.
    vector<kalmanType> tempArray;
    for(vector<kalmanType>::size_type  i = 0; i < myParamVec.size() ; i++)
    {
	tempArray.push_back(myParamVec[i]);

    }
    vector<kalmanType> tempVecS;
    vector<kalmanType> tempVecTheta;
    vector<kalmanType> tempVecT1;
    vector<kalmanType> tempVecT2;

    for(vector<kalmanType>::size_type  i = 0; i < (WINDOW_SIZE) ; i++)
    {

	tempVecS.push_back(tempArray[i*4]);
	tempVecTheta.push_back(tempArray[i*4 + 1]);
	tempVecT1.push_back(tempArray[i*4 + 2]);
	tempVecT2.push_back(tempArray[i*4 + 3]);
    }
    // Now make one array with WINDOW_SIZE+1 and put the stuff inside.
    vector<kalmanType> copyVecS;
    vector<kalmanType> copyVecTheta;
    vector<kalmanType> copyVecT1;
    vector<kalmanType> copyVecT2;
    for(vector<kalmanType>::size_type  i = 0; i < (WINDOW_SIZE) ; i++)
    {
	copyVecS.push_back(tempVecS[i]);
	copyVecTheta.push_back(tempVecTheta[i]);
	copyVecT1.push_back(tempVecT1[i]);
	copyVecT2.push_back(tempVecT2[i]);
    }
    // append one more entry.
    copyVecS.push_back(1);
    copyVecTheta.push_back(0);
    copyVecT1.push_back(0);
    copyVecT2.push_back(0);

    //kalmanType thisS;
    kalmanType thisTheta;
    kalmanType thisT[2];

    //kalmanType finalS;
    //kalmanType finalTheta;
    //kalmanType finalT[2];


    for(vector<kalmanType>::size_type  i = WINDOW_SIZE+1; i > 1 ; i--)
    {
	copyVecS[i-2]= copyVecS[i-1]*tempVecS[i-2]; 
	copyVecTheta[i-2]= copyVecTheta[i-1] + tempVecTheta[i-2];
	thisTheta = copyVecTheta[i-2];
	//thisS = copyVecS[i-2];

	kalmanType thisTtoAdd[2];
	kalmanType thisTtoMult[2];

	thisTtoAdd[0] = copyVecT1[i-1];
	thisTtoAdd[1] = copyVecT2[i-1];
	thisTtoMult[0] = tempVecT1[i-2];
	thisTtoMult[1] = tempVecT2[i-2];

	kalmanType mat[4];
	mat[0] = cos(thisTheta);
	mat[1] = -sin(thisTheta);
	mat[2] = sin(thisTheta);
	mat[3] = cos(thisTheta);
#if 0
	cout<<"\n\n\t Mat:"<<mat[0]<<"\t"<<mat[1]<<"\t"<<mat[2]<<"\t"<<mat[3];
	cout<<"\n\t Tadd :"<<thisTtoAdd[0]<<"\t"<<thisTtoAdd[1];
	cout<<"\n\t Tmult:"<<thisTtoMult[0]<<"\t"<<thisTtoMult[1];
	cout<<"\n\t multS: "<<tempVecS[i-2];

#endif
	kalmanType tempMult[2];
	tempMult[0] = tempVecS[i-2]*(mat[0]*thisTtoMult[0] + mat[1]*thisTtoMult[1]);
	tempMult[1] = tempVecS[i-2]*(mat[2]*thisTtoMult[0] + mat[3]*thisTtoMult[1]);

	thisT[0] = tempMult[0] + thisTtoAdd[0];
	thisT[1] = tempMult[1] + thisTtoAdd[1];

	copyVecT1[i-2]= copyVecT1[i-1] + thisT[0];
	copyVecT2[i-2]= copyVecT2[i-1] + thisT[1];

    }

    /*
     * Now push these values into a array to be used for filterning.
     */

    // construct the vector to hold the values.
    vector<kalmanType> filterVec;

    // Push the values into it.
    for(vector<kalmanType>::size_type  i = 0; i < copyVecS.size() - 1 ; i++)
    {
	filterVec.push_back(copyVecS[i]);
	filterVec.push_back(copyVecTheta[i]);
	filterVec.push_back(copyVecT1[i]);
	filterVec.push_back(copyVecT2[i]);
    }

    if(filterVec.size() >= 4*FILTER_SIZE)
    {
	while(filterVec.size() > 4*FILTER_SIZE)
	{
	    filterVec.erase(filterVec.begin());
	}
    }
    else
    {   
	while(filterVec.size() < 4*FILTER_SIZE)
	{
	    filterVec.push_back(0);
	}
    }

    // Filter it.
    myFilteredVec[0]= 0;
    myFilteredVec[1]= 0;
    myFilteredVec[2]= 0;
    myFilteredVec[3]= 0;
    for(vector<kalmanType>::size_type  i = FILTER_SIZE -1; i > 0 ; i--)
    {
	img_stab_if_R::myFilteredVec[0] = img_stab_if_R::myFilteredVec[0]
	    + filterVec[4*i];
	img_stab_if_R::myFilteredVec[1] = img_stab_if_R::myFilteredVec[1]
	    + filterVec[4*i + 1];
	img_stab_if_R::myFilteredVec[2] = img_stab_if_R::myFilteredVec[2]
	    + filterVec[4*i + 2];
	img_stab_if_R::myFilteredVec[3] = img_stab_if_R::myFilteredVec[3]
	    + filterVec[4*i + 3];

    }
    img_stab_if_R::myFilteredVec[0] = 
	img_stab_if_R::myFilteredVec[0]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[1] = 
	img_stab_if_R::myFilteredVec[1]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[2] = 
	img_stab_if_R::myFilteredVec[2]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[3] = 
	img_stab_if_R::myFilteredVec[3]/FILTER_SIZE;


#if 0

    /*
      cout<<"\nThisS "<<copyVecS[0];
      cout<<"\nThisTheta "<<copyVecTheta[0];
      cout<<"\nThisTheta "<<copyVecT1[0]<<copyVecT2[0];
      */

    for(vector<kalmanType>::size_type  i = 0; i < tempVecS.size() ; i++)
    {
	cout<<"\tv"<<copyVecS[i]<<"\t";
    }

    for(vector<kalmanType>::size_type  i = 0; i < tempVecTheta.size() ; i++)
    {
	cout<<"\tv"<<copyVecTheta[i]<<"\t";
    }

    for(vector<kalmanType>::size_type  i = 0; i < tempVecT1.size() ; i++)
    {
	cout<<"\tv"<<tempVecT1[i]<<"\t";
    }



    cout<<"\n\nDebug : size ->"<<tempVecS.size()<<"\t"<<tempArray.size();

    cout<<"\n--DEBUG: Printing s values."<<endl;
    for(vector<kalmanType>::size_type  i =0; i < tempVecS.size() ; i++)
    {
	cout<<"\t"<<myVecS[i];
    }
#endif

    return img_stab_if_R::myFilteredVec;
}


/*******************************************************************************
 * This Function run the whole IS module.
 *******************************************************************************/

void img_stab_if_R::runImageStabilizeR(void) 
{
    //int cnt;

    //ULONG lastVsyncTimeStamp;
    //ULONG lastMotionTimeStamp;
    bool ifChangeInQuats = true;

    /* Here we update the arrayQuaternions */
    for(int i = 0; i < QUAT_LENGTH; i ++)
    {
	arrayTwoQuaternion[i] = arrayTwoQuaternion[QUAT_LENGTH + i]; // pervious Quaternion
	// = current Quaternion
    }

    /*
     * Here, push the corrected quaternion into queue. This queue always contains current 
     * quaternion as well as previous quaternion.
     */

    for(int i = 0; i < QUAT_LENGTH; i++)
    {
	arrayTwoQuaternion[QUAT_LENGTH + i] = pObjCamMotionManager->pObjKalmanFilter->correctedQuaternion_win[i];
    }

    /* 
     * If there is no change in quaternions. return.
     */
    for(int i = 0; i < QUAT_LENGTH; i++)
    {
	if(arrayTwoQuaternion[i] == arrayTwoQuaternion[QUAT_LENGTH+i])
	{
	    ifChangeInQuats = false;
	    return;
	}
    }

    /*
      TO DO: Extrapolate the quaternion using time stamp
      */


    /*
     * Create an array of camera matrix.
     * For testing : One can use artificial camera matrix.
     */
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
	arrayCamInternalMatrix[i] = arrayCamInternalMatrix[K_MATRIX_LENGTH + i];
    }
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
	arrayCamInternalMatrix[K_MATRIX_LENGTH + i] = pObjCamMotionManager->pObjCamMotion->camInternalMat[i];
    }

    /*
     * CALLING FUNCTIONS to CALCULATE THE s, theta, and T.
     */

    //kalmanType* pA;
    //kalmanType* pQ;
    if(true == ifChangeInQuats)
    {   
	IsHomographyR::getHomographyMat(arrayTwoQuaternion, arrayCamInternalMatrix);
	IsHomographyR::decomposeHomographyMat(IsHomographyR::imgHomographyMat);
	/*
	 * this call will accumulate all the parameters into a queue of windowSize values.
	 */

	img_stab_if_R::myParamVec = img_stab_if_R::accParamsInQueue(img_stab_if_R::imgSThetaTArray
		, img_stab_if_R::myParamVec);

	/*
	 * This call will filter the values. filterLength can be changes in macro.
	 */
	img_stab_if_R::myFilteredVec = img_stab_if_R::calcSmoothMotion(img_stab_if_R::myParamVec);
    }

    /*
     * Debug : Print the values here
     */

#if 1
    /*
     * This part of the code will adjust the cropping parameters. The image is to
     * be cropped using the translation vector. Using theta (i.e. rotation of the
     * image) is not possible due to hardware limitations.
     * IT ONLY RUNS IF THERE IS ANY CHANGE IN QUATERNIONS.
     */

    /*
     * put the translation vector in a separete variable.
     */
    double transVec_x = imgSThetaTArray[2];
    double transVec_y = imgSThetaTArray[3];
    double centre_x = 500;  // Change the values.
    double centre_y = 500;  // Change the values.
    /*
     * Define a relation between screen resolution and translation vector.
     * ToDO : Make a macro.
     */
    if(true == ifChangeInQuats)
    {
	double scaleFactorScrnToTransVec = 1;
	double moveCentre_x = centre_x - transVec_x*scaleFactorScrnToTransVec;
	double moveCentre_y = centre_y - transVec_y*scaleFactorScrnToTransVec;

	/*
	 * call the function which update the cropping parameters.
	 */
	UINT8 ucMonplId = 0;
	monpl_adjustCropParameter(moveCentre_x, moveCentre_y, ucMonplId);
    }
#endif

}

